
#include "SubdivisionGeom.h"
#include "Entity.h"
#include "Engine.h"
//#include "boost/Thread/Thread.hpp"

//
// Constructor
//

SubdivisionGeom::SubdivisionGeom(Entity *_parent) : Geometry(_parent)
{
	subLevel = 0;
	isSubdivisionGeom = true;
}

//
// Destructor
//

SubdivisionGeom::~SubdivisionGeom()
{
	for(uint i = 0; i < subMeshes.size(); ++i)
		subMeshes[i].CleanUp();
}

//
// _Render
//

void SubdivisionGeom::_Render(uchar renderFlag)
{
	// Render the meshes
	if(subLevel == 0)
	{
		mesh.CreateBuffers();
		mesh.LoadBuffers(renderFlag);
	}
	else
	{
		subMeshes[subLevel-1].CreateBuffers();
		subMeshes[subLevel-1].LoadBuffers(renderFlag);
	}
}

//
// Clear
//

void SubdivisionGeom::Clear()
{
	// Clears Geometry data
	mesh.Clear();

	// Clears Subdivision data
	for(uint i = 0; i < subMeshes.size(); ++i)
		subMeshes[i].Clear();

	// Reset subLevel
	subLevel = 0;
}

//
// AddNormals
//

void SubdivisionGeom::AddNormals()
{
	if(updateNormals)
	{
		uint s = normals.size();
		if(subLevel != 0)
		{
			normals.resize(s + subMeshes[subLevel-1].vertices.size());

			for(uint i = 0; i < subMeshes[subLevel-1].vertices.size(); ++i)
				normals[s+i].Set(subMeshes[subLevel-1].vertices[i].position, 
												 subMeshes[subLevel-1].vertices[i].position + subMeshes[subLevel-1].vertices[i].normal * NORMAL_MODIFIER);
		}
		else
		{
			normals.resize(s + mesh.vertices.size());

			for(uint i = 0; i < mesh.vertices.size(); ++i)
				normals[s+i].Set(mesh.vertices[i].position, 
				                 mesh.vertices[i].position + mesh.vertices[i].normal * NORMAL_MODIFIER);
		}
	}
}

//
// DrawGrid
//

void SubdivisionGeom::DrawGrid(uint width, uint height, float size) 
{
	_DrawGrid(&mesh, width, height, size);
}

uint GetPow2(uint max)
{
	uint count = 0;
	while(max != 0)
	{
		max = max >> 1;
		count++;
	}

	return 1 << (count-1);
}

//
// DrawHeightMap
//

#define GRID_SQUARE 1.0f

void SubdivisionGeom::DrawHeightMap(vector<vector<uchar> > &hMap, float scale)
{
	uint size = hMap.size();
	uint newSize = GetPow2(size-1);

	// Create grid
	DrawGrid(newSize, newSize, GRID_SQUARE);
	newSize++;

	for(uint i = 0; i < newSize; ++i)
	for(uint j = 0; j < newSize; ++j)
	{
		mesh.vertices[i*newSize+j].position.y = scale * hMap[i][j];
		parent->AddBBoxPoint(mesh.vertices[i*newSize+j].position);
	}

	// Calculate some normals
	mesh.CalculateVertexNormals();

	// Generates subMeshes
	GenerateHeightMapSubMeshes((newSize-1)*GRID_SQUARE);
}

//
// DrawBox
//

#define SQRT3I 0.557350269f 

void SubdivisionGeom::DrawBox(float s)
{
	s *= 0.5f;

	ushort v0 = AddVertex(Real3( s,  s,  s), Real3( SQRT3I,  SQRT3I,  SQRT3I), Real2(1.0f, 0.0f));
	ushort v1 = AddVertex(Real3( s,  s, -s), Real3( SQRT3I,  SQRT3I, -SQRT3I), Real2(1.0f, 1.0f));
	ushort v2 = AddVertex(Real3(-s,  s, -s), Real3(-SQRT3I,  SQRT3I, -SQRT3I), Real2(0.0f, 1.0f));
	ushort v3 = AddVertex(Real3(-s,  s,  s), Real3(-SQRT3I,  SQRT3I,  SQRT3I), Real2(0.0f, 0.0f));
	
	ushort v4 = AddVertex(Real3( s, -s,  s), Real3( SQRT3I, -SQRT3I,  SQRT3I), Real2(1.0f, 1.0f));
	ushort v5 = AddVertex(Real3( s, -s, -s), Real3( SQRT3I, -SQRT3I, -SQRT3I), Real2(0.0f, 1.0f));
	ushort v6 = AddVertex(Real3(-s, -s, -s), Real3(-SQRT3I, -SQRT3I, -SQRT3I), Real2(0.0f, 0.0f));
	ushort v7 = AddVertex(Real3(-s, -s,  s), Real3(-SQRT3I, -SQRT3I,  SQRT3I), Real2(1.0f, 0.0f));

	AddQuad(Quad(v0, v1, v2, v3));
	AddQuad(Quad(v0, v4, v5, v1));
	AddQuad(Quad(v1, v5, v6, v2));
	AddQuad(Quad(v2, v6, v7, v3));
	AddQuad(Quad(v0, v3, v7, v4));
	AddQuad(Quad(v4, v7, v6, v5));
}

//
// AddVertex
//

int SubdivisionGeom::AddVertex(Vertex &v)
{
	parent->AddBBoxPoint(v.position);
	return mesh.AddVertex(v);
}

int SubdivisionGeom::AddVertex(Real3 position, Real3 normal, Real2 tex)
{
	return AddVertex(Vertex(position, normal, tex));
}

//
// AddUniqueVertex
//

int SubdivisionGeom::AddUniqueVertex(Real3 position, Real3 normal, Real2 tex)
{
	parent->AddBBoxPoint(position);
	return mesh.AddUniqueVertex(position, normal, tex);
}

//
// AddTriangle
//

int SubdivisionGeom::AddTriangle(Triangle &t)
{
	return mesh.AddTriangle(t);
}

int SubdivisionGeom::AddTriangle(uint a, uint b, uint c)
{
	return mesh.AddTriangle(Triangle(a, b, c));
}

//
// AddQuad
//

int SubdivisionGeom::AddQuad(Quad &q)
{
	return mesh.AddQuad(q);
}

int SubdivisionGeom::AddQuad(uint a, uint b, uint c, uint d)
{
	return mesh.AddQuad(Quad(a, b, c, d));
}

//
// Subdivide
//

void SubdivisionGeom::IncreaseSubLevel()
{
	//boost::lock_guard<boost::mutex> lock(mSubdividing);
	if(subLevel + 1 > SUB_LEVEL_MAX)
		return;

	if(subLevel + 1 > (uchar)subMeshes.size())
		CreateSubdivision(subLevel);

	++subLevel;
}

void SubdivisionGeom::DecreaseSubLevel()
{
	//boost::lock_guard<boost::mutex> lock(mSubdividing);
	if(subLevel == 0)
		return;

	--subLevel;
}

//
// Cleans up if subdivide stopped in middle
//

void SubdivisionGeom::CleanPartialSubMesh()
{
	subMeshes.pop_back();
	--subLevel;
}

//
// Helper Vertex Class
//

class _Vertex
{
public:

	vector<Real3> fPoints;
	vector<Real3> ePoints;

	void AddFPoint(const Real3 &f)
	{
		fPoints.push_back(f);
	}

	void AddEPoint(const Real3 &e)
	{
		for(uint i = 0; i < ePoints.size(); ++i)
			if(ePoints[i] == e) return;
		ePoints.push_back(e);
	}
};

//
// Subdivide
//

#define USE_TIMER
//#define UPDATE_SCREEN
#define MAX_SUB_TIME 3000
#define SMOOTH_SUBDIVIDED
#define CHECK_SIZE(X) if((X-vSize)+1 > normals.size()) normals.push_back(empty);

bool SubdivisionGeom::CreateSubdivision(ushort level)
{
	// Normals for the newly created vertices
	vector< vector<Real3> > normals;
	vector<Real3> empty;

	// Midpoint data for old triangles
	#ifdef SMOOTH_SUBDIVIDED
	vector<_Vertex> vMidPoints;
	#endif

	#ifdef USE_TIMER
	uint timer = 0;
	#endif

	uint vSize = (level == 0) ? mesh.vertices.size() : subMeshes[level-1].vertices.size();

	// Check max vertices
	if(vSize > MAX_VERTICES)
	{
		--subLevel;
		return false;
	}

	// Add new subdivision mesh
	subMeshes.push_back(Mesh());

	if(level == 0)
	{
		uint tSize = mesh.triangles.size();
		uint qSize = mesh.quads.size();

		// Copy over old vertices
		subMeshes[level].vertices = mesh.vertices;

		// Midpoint data for old triangles
		#ifdef SMOOTH_SUBDIVIDED
		vMidPoints.resize(vSize);
		#endif

		for(uint i = 0; i < tSize; ++i)
		{
			// New Vertices
			ushort _a = subMeshes[level].AddUniqueVertex(mesh.triangles[i].Side0MidPoint(&mesh.vertices), mesh.triangles[i].Side0Normal(&mesh.vertices), mesh.triangles[i].Side0Tex(&mesh.vertices));
			ushort _b = subMeshes[level].AddUniqueVertex(mesh.triangles[i].Side1MidPoint(&mesh.vertices), mesh.triangles[i].Side1Normal(&mesh.vertices), mesh.triangles[i].Side1Tex(&mesh.vertices));
			ushort _c = subMeshes[level].AddUniqueVertex(mesh.triangles[i].Side2MidPoint(&mesh.vertices), mesh.triangles[i].Side2Normal(&mesh.vertices), mesh.triangles[i].Side2Tex(&mesh.vertices));
			ushort _d = subMeshes[level].AddUniqueVertex(mesh.triangles[i].MidPoint(&mesh.vertices),      mesh.triangles[i].Normal(&mesh.vertices),      mesh.triangles[i].Tex(&mesh.vertices));

			// Mid Points
			#ifdef SMOOTH_SUBDIVIDED
			Real3 m = mesh.triangles[i].MidPoint(&mesh.vertices);
			vMidPoints[mesh.triangles[i].a].AddFPoint(m);
			vMidPoints[mesh.triangles[i].a].AddEPoint(mesh.triangles[i].Side0MidPoint(&mesh.vertices));
			vMidPoints[mesh.triangles[i].a].AddEPoint(mesh.triangles[i].Side2MidPoint(&mesh.vertices));
			vMidPoints[mesh.triangles[i].b].AddFPoint(m);
			vMidPoints[mesh.triangles[i].b].AddEPoint(mesh.triangles[i].Side0MidPoint(&mesh.vertices));
			vMidPoints[mesh.triangles[i].b].AddEPoint(mesh.triangles[i].Side1MidPoint(&mesh.vertices));
			vMidPoints[mesh.triangles[i].c].AddFPoint(m);
			vMidPoints[mesh.triangles[i].c].AddEPoint(mesh.triangles[i].Side1MidPoint(&mesh.vertices));
			vMidPoints[mesh.triangles[i].c].AddEPoint(mesh.triangles[i].Side2MidPoint(&mesh.vertices));
			#endif

			// Normals
			CHECK_SIZE(_a); normals[_a-vSize].push_back(mesh.triangles[i].Side0Normal(&mesh.vertices));
			CHECK_SIZE(_b); normals[_b-vSize].push_back(mesh.triangles[i].Side1Normal(&mesh.vertices));
			CHECK_SIZE(_c); normals[_c-vSize].push_back(mesh.triangles[i].Side2Normal(&mesh.vertices));
			CHECK_SIZE(_d); normals[_d-vSize].push_back(mesh.triangles[i].Normal(&mesh.vertices));

			// Create new triangles 
			subMeshes[level].AddQuad(mesh.triangles[i].a, _a, _d, _c);
			subMeshes[level].AddQuad(_a, mesh.triangles[i].b, _b, _d);
			subMeshes[level].AddQuad(_c, _d, _b, mesh.triangles[i].c);

			// Check time
			#ifdef UPDATE_SCREEN
			if(GetTickCount() & 1023)
				engine.Render();
			#endif
			#ifdef USE_TIMER
			if(++timer & 0x00000fff)
			{
				timer = 0;
				if(GetTickCount() - engine.lastTime > MAX_SUB_TIME)
				{
					CleanPartialSubMesh();
					return false;
				}
			}
			#endif
		}

		for(uint i = 0; i < qSize; ++i)
		{
			// New Vertices
			ushort _a = subMeshes[level].AddUniqueVertex(mesh.quads[i].Side0MidPoint(&mesh.vertices), mesh.quads[i].Side0Normal(&mesh.vertices), mesh.quads[i].Side0Tex(&mesh.vertices));
			ushort _b = subMeshes[level].AddUniqueVertex(mesh.quads[i].Side1MidPoint(&mesh.vertices), mesh.quads[i].Side1Normal(&mesh.vertices), mesh.quads[i].Side1Tex(&mesh.vertices));
			ushort _c = subMeshes[level].AddUniqueVertex(mesh.quads[i].Side2MidPoint(&mesh.vertices), mesh.quads[i].Side2Normal(&mesh.vertices), mesh.quads[i].Side2Tex(&mesh.vertices));
			ushort _d = subMeshes[level].AddUniqueVertex(mesh.quads[i].Side3MidPoint(&mesh.vertices), mesh.quads[i].Side3Normal(&mesh.vertices), mesh.quads[i].Side3Tex(&mesh.vertices));
			ushort _e = subMeshes[level].AddUniqueVertex(mesh.quads[i].MidPoint(&mesh.vertices),      mesh.quads[i].Normal(&mesh.vertices),      mesh.quads[i].Tex(&mesh.vertices));

			// Mid Points
			#ifdef SMOOTH_SUBDIVIDED
			Real3 m = mesh.quads[i].MidPoint(&mesh.vertices);
			vMidPoints[mesh.quads[i].a].AddFPoint(m);
			vMidPoints[mesh.quads[i].a].AddEPoint(mesh.quads[i].Side0MidPoint(&mesh.vertices));
			vMidPoints[mesh.quads[i].a].AddEPoint(mesh.quads[i].Side3MidPoint(&mesh.vertices));
			vMidPoints[mesh.quads[i].b].AddFPoint(m);
			vMidPoints[mesh.quads[i].b].AddEPoint(mesh.quads[i].Side0MidPoint(&mesh.vertices));
			vMidPoints[mesh.quads[i].b].AddEPoint(mesh.quads[i].Side1MidPoint(&mesh.vertices));
			vMidPoints[mesh.quads[i].c].AddFPoint(m);
			vMidPoints[mesh.quads[i].c].AddEPoint(mesh.quads[i].Side1MidPoint(&mesh.vertices));
			vMidPoints[mesh.quads[i].c].AddEPoint(mesh.quads[i].Side2MidPoint(&mesh.vertices));
			vMidPoints[mesh.quads[i].d].AddFPoint(m);
			vMidPoints[mesh.quads[i].d].AddEPoint(mesh.quads[i].Side2MidPoint(&mesh.vertices));
			vMidPoints[mesh.quads[i].d].AddEPoint(mesh.quads[i].Side3MidPoint(&mesh.vertices));
			#endif

			// Normals
			CHECK_SIZE(_a); normals[_a-vSize].push_back(mesh.quads[i].Side0Normal(&mesh.vertices));
			CHECK_SIZE(_b); normals[_b-vSize].push_back(mesh.quads[i].Side1Normal(&mesh.vertices));
			CHECK_SIZE(_c); normals[_c-vSize].push_back(mesh.quads[i].Side2Normal(&mesh.vertices));
			CHECK_SIZE(_d); normals[_d-vSize].push_back(mesh.quads[i].Side3Normal(&mesh.vertices));
			CHECK_SIZE(_e); normals[_e-vSize].push_back(mesh.quads[i].Normal(&mesh.vertices));

			// Create new triangles 
			subMeshes[level].AddQuad(mesh.quads[i].a, _a, _e, _d);
			subMeshes[level].AddQuad(_a, mesh.quads[i].b, _b, _e);
			subMeshes[level].AddQuad(_e, _b, mesh.quads[i].c, _c);
			subMeshes[level].AddQuad(_d, _e, _c, mesh.quads[i].d);

			// Check time
			#ifdef UPDATE_SCREEN
			if(GetTickCount() & 1023)
				engine.Render();
			#endif
			#ifdef USE_TIMER
			if(++timer & 0x00000fff)
			{
				timer = 0;
				if(GetTickCount() - engine.lastTime > MAX_SUB_TIME)
				{
					CleanPartialSubMesh();
					return false;
				}
			}
			#endif
		}
	}
	else
	{
		uint qSize = subMeshes[level-1].quads.size();

		// Copy over old vertices
		subMeshes[level].vertices = subMeshes[level-1].vertices;

		// Midpoint data for old triangles
		#ifdef SMOOTH_SUBDIVIDED
		vMidPoints.resize(vSize);
		#endif

		for(uint i = 0; i < qSize; ++i)
		{
			// New Vertices
			ushort _a = subMeshes[level].AddUniqueVertex(subMeshes[level-1].quads[i].Side0MidPoint(&subMeshes[level-1].vertices), subMeshes[level-1].quads[i].Side0Normal(&subMeshes[level-1].vertices), subMeshes[level-1].quads[i].Side0Tex(&subMeshes[level-1].vertices));
			ushort _b = subMeshes[level].AddUniqueVertex(subMeshes[level-1].quads[i].Side1MidPoint(&subMeshes[level-1].vertices), subMeshes[level-1].quads[i].Side1Normal(&subMeshes[level-1].vertices), subMeshes[level-1].quads[i].Side1Tex(&subMeshes[level-1].vertices));
			ushort _c = subMeshes[level].AddUniqueVertex(subMeshes[level-1].quads[i].Side2MidPoint(&subMeshes[level-1].vertices), subMeshes[level-1].quads[i].Side2Normal(&subMeshes[level-1].vertices), subMeshes[level-1].quads[i].Side2Tex(&subMeshes[level-1].vertices));
			ushort _d = subMeshes[level].AddUniqueVertex(subMeshes[level-1].quads[i].Side3MidPoint(&subMeshes[level-1].vertices), subMeshes[level-1].quads[i].Side3Normal(&subMeshes[level-1].vertices), subMeshes[level-1].quads[i].Side3Tex(&subMeshes[level-1].vertices));
			ushort _e = subMeshes[level].AddUniqueVertex(subMeshes[level-1].quads[i].MidPoint(&subMeshes[level-1].vertices),      subMeshes[level-1].quads[i].Normal(&subMeshes[level-1].vertices),      subMeshes[level-1].quads[i].Tex(&subMeshes[level-1].vertices));

			// Mid Points
			#ifdef SMOOTH_SUBDIVIDED
			Real3 m = subMeshes[level-1].quads[i].MidPoint(&subMeshes[level-1].vertices);
			vMidPoints[subMeshes[level-1].quads[i].a].AddFPoint(m);
			vMidPoints[subMeshes[level-1].quads[i].a].AddEPoint(subMeshes[level-1].quads[i].Side0MidPoint(&subMeshes[level].vertices));
			vMidPoints[subMeshes[level-1].quads[i].a].AddEPoint(subMeshes[level-1].quads[i].Side3MidPoint(&subMeshes[level].vertices));
			vMidPoints[subMeshes[level-1].quads[i].b].AddFPoint(m);
			vMidPoints[subMeshes[level-1].quads[i].b].AddEPoint(subMeshes[level-1].quads[i].Side0MidPoint(&subMeshes[level].vertices));
			vMidPoints[subMeshes[level-1].quads[i].b].AddEPoint(subMeshes[level-1].quads[i].Side1MidPoint(&subMeshes[level].vertices));
			vMidPoints[subMeshes[level-1].quads[i].c].AddFPoint(m);
			vMidPoints[subMeshes[level-1].quads[i].c].AddEPoint(subMeshes[level-1].quads[i].Side1MidPoint(&subMeshes[level].vertices));
			vMidPoints[subMeshes[level-1].quads[i].c].AddEPoint(subMeshes[level-1].quads[i].Side2MidPoint(&subMeshes[level].vertices));
			vMidPoints[subMeshes[level-1].quads[i].d].AddFPoint(m);
			vMidPoints[subMeshes[level-1].quads[i].d].AddEPoint(subMeshes[level-1].quads[i].Side2MidPoint(&subMeshes[level].vertices));
			vMidPoints[subMeshes[level-1].quads[i].d].AddEPoint(subMeshes[level-1].quads[i].Side3MidPoint(&subMeshes[level].vertices));
			#endif

			// Normals
			CHECK_SIZE(_a); normals[_a-vSize].push_back(subMeshes[level-1].quads[i].Side0Normal(&subMeshes[level-1].vertices));
			CHECK_SIZE(_b); normals[_b-vSize].push_back(subMeshes[level-1].quads[i].Side1Normal(&subMeshes[level-1].vertices));
			CHECK_SIZE(_c); normals[_c-vSize].push_back(subMeshes[level-1].quads[i].Side2Normal(&subMeshes[level-1].vertices));
			CHECK_SIZE(_d); normals[_d-vSize].push_back(subMeshes[level-1].quads[i].Side3Normal(&subMeshes[level-1].vertices));
			CHECK_SIZE(_e); normals[_e-vSize].push_back(subMeshes[level-1].quads[i].Normal(&subMeshes[level-1].vertices));

			// Create new triangles 
			subMeshes[level].AddQuad(subMeshes[level-1].quads[i].a, _a, _e, _d);
			subMeshes[level].AddQuad(_a, subMeshes[level-1].quads[i].b, _b, _e);
			subMeshes[level].AddQuad(_e, _b, subMeshes[level-1].quads[i].c, _c);
			subMeshes[level].AddQuad(_d, _e, _c, subMeshes[level-1].quads[i].d);

			// Check time
			#ifdef UPDATE_SCREEN
			if(GetTickCount() & 1023)
				engine.Render();
			#endif
			#ifdef USE_TIMER
			if(++timer & 0x00000fff)
			{
				timer = 0;
				if(GetTickCount() - engine.lastTime > MAX_SUB_TIME)
				{
					CleanPartialSubMesh();
					return false;
				}
			}
			#endif
		}
	}

	for(uint i = 0; i < normals.size(); ++i)
	{
		Real3 total;
		for(uint j = 0; j < normals[i].size(); ++j)
			total += normals[i][j];
		total *= (1.0f / normals[i].size());

		subMeshes[level].vertices[i+vSize].normal = total;
	}

	#ifdef SMOOTH_SUBDIVIDED
	for(uint i = 0; i < vSize; ++i)
	{
		Real3 F;
		Real3 R;

		for(uint j = 0; j < vMidPoints[i].fPoints.size(); ++j)
		{
			F += vMidPoints[i].fPoints[j];
			R += vMidPoints[i].ePoints[j];
		}

		float iSize = 1.0f / vMidPoints[i].fPoints.size();

		F *= iSize;
		R *= iSize;

		subMeshes[level].vertices[i].position = iSize * (F + 2.0f*R + (vMidPoints[i].fPoints.size() - 3.0f) * subMeshes[level].vertices[i].position);
	}
	#endif

	return true;
}

//
// GenerateHeightMapSubMeshes
//

void SubdivisionGeom::GenerateHeightMapSubMeshes(float sideLength)
{
	subMeshes.resize(5);
	subMeshes[4] = mesh;

	uint quads = (uint)sqrt((float)mesh.quads.size());

	mesh.Clear();

	for(short i = 4; i >= 0; --i)
	{
		Mesh *m = (i == 0 ? &mesh : &subMeshes[i-1]);
		Mesh *p = &subMeshes[i];
		quads = quads >> 1;

		_DrawGrid(m, quads, quads, sideLength / (float)quads);

		uint q = quads*2+1;

		for(uint j = 0; j < quads+1; ++j)
		for(uint k = 0; k < quads+1; ++k)
		{
			m->vertices[j*(quads+1)+k].position.y = p->vertices[(j*2)*q+(k*2)].position.y;
			m->vertices[j*(quads+1)+k].normal     = p->vertices[(j*2)*q+(k*2)].normal;
		}
	}
}

//
// Rotate
//

void SubdivisionGeom::Rotate(const D3DXMATRIX *rotation)
{
	mesh.RotateVertices(rotation);
	for(ushort i = 0; i < subMeshes.size(); ++i)
		subMeshes[i].RotateVertices(rotation);
}